#include "headfile.h"
#include "mpu_prepare.h"

//================================================================//
//  @brief  :		延距清环岛7状态标志位
//  @param  :		void
//  @return :		void
//  @note   :		放在中断
//================================================================//
void Dist_ClearIslandSeven(void)
{
	static int sum_dist = ONE_METER/2;
	static int acc_dist = 0;
	if (CircleState>=6)
	{
		acc_dist += CurSpeed;
		if (acc_dist > sum_dist)
		{
			Dist_ClearSevenFlag = 1;
		}
		else Dist_ClearSevenFlag = 0;
	}
	else
	{
		Dist_ClearSevenFlag = 0;
		acc_dist = 0;
	}
}

//================================================================//
//  @brief  :		角速度积分跳环岛6状态
//  @param  :		void
//  @return :		void
//  @note   :		放在中断
//================================================================//
void Angle_GoIslandSix(void)
{
	static float acc_angle = 0;
	if (CL == CircleFlag && 7 > CircleState)
	{
		acc_angle += sensor.Gyro_deg.z * 0.002;
		if (acc_angle > 310)
		{
			if (CircleState < 6) CircleState = 7;
			acc_angle = 0;
		}
		else if (acc_angle < 280)
		{
			if (CircleState > 5) CircleState = 5;
		}
	}
	else if (CR == CircleFlag && 6 > CircleState)
	{
		acc_angle += sensor.Gyro_deg.z * 0.002;
		if (acc_angle < -310)
		{
			if (CircleState < 7) CircleState = 7;
			acc_angle = 0;
		}
		else if (acc_angle > -280)
		{
			if (CircleState > 5) CircleState = 5;
		}
	}
	else acc_angle = 0;
}

//================================================================//
//  @brief  :		延距清除错误环岛标志
//  @param  :		void
//  @return :		void
//  @note   :		放在中断
//================================================================//
void DistCI(void)
{
	static int CI_sum_dist = 0.6 * ONE_METER;
	static int CI_acc_dist = 0;
	static int state = 0;
	switch (state)
	{
	case 0:
		if (CircleFlag && 7 != CircleState)
			state = 1;
		break;
	case 1:
		CI_acc_dist += CurSpeed;
		if (CI_acc_dist > CI_sum_dist)
		{
			state = 0;
			CircleFlag = 0;
			CircleState = 0;
			CI_acc_dist = 0;
		}
		else if (IndJudgeIntoCircle(0))
		{
			if (CircleState > 3) CircleState = 3;
			state = 2;
			CI_acc_dist = 0;
		}
		break;
	case 2:
		if (7 == CircleState)
		{
			state = 0;
		}
		break;
	default:
		break;
	}
}

//================================================================//
//  @brief  :		陀螺仪辅助出三岔
//  @param  :		void
//  @return :		void
//  @note   :		void
//================================================================//
void Angle_GoForksZero(void)
{
	static float acc_angle = 0;
	if (Forks_State&&Forks_Direct==FL)
	{
		acc_angle += sensor.Gyro_deg.z * 0.002;
		if (acc_angle > 0)
		{
			if (Forks_State >2) Forks_State=0;
			acc_angle = 0;
		}
		else if (acc_angle < -5)
		{
			if (!Forks_State) Forks_State = 5;
		}
	}
	else if (Forks_State&&Forks_Direct==FR)
	{
		acc_angle += sensor.Gyro_deg.z * 0.002;
		if (acc_angle <0)
		{
			if (Forks_State > 2) Forks_State = 0;
			acc_angle = 0;
		}
		else if (acc_angle > 5)
		{
			if (!Forks_State ) Forks_State = 5;
		}
	}
	else acc_angle = 0;
    //    acc_copy=acc_angle;
}

//================================================================//
//  @brief  :		延距清Forks 3状态
//  @param  :		void
//  @return :		void
//  @note   :		放在中断
//================================================================//
void Dist_ClearForks3Flag(void)
{
	static int sum_dist = 2*ONE_METER;
	static int acc_dist = 0;
	if (Forks_State==3)
	{
		acc_dist += CurSpeed;
		if (acc_dist > sum_dist)
		{
			Forks_State=0;
			Dist_Forks3Flag=0;
		}
		else ;
	}
	else
	{
		acc_dist = 0;
	}
}